#include "clkWorld.h"

clkWorld::clkWorld()
{

}

clkWorld::~clkWorld()
{
	//implicitly destroy
	destroy();
}

void clkWorld::init(float a_cellWidth, float a_cellHeight)
{
	//initializes the partitioner with the given cell width and height
	m_partition.init(a_cellWidth,a_cellHeight);
}

void clkWorld::destroy()
{
	//loop; destroy and delete object
	for(int i = 0; i < m_objects.size(); i++)
	{
		m_objects[i]->destroy();
		SAFE_DELETE(m_objects[i]);
	}

	//release object list
	m_objects.release();

	//destroy partitioner
	m_partition.destroy();
}

void clkWorld::updatePhysics(float a_time)
{
	//clear partitioner
	m_partition.clearCells();

	//loop and update physics on all objects
	for(int i = 0; i < m_objects.size(); i++)
	{
		//grab object properties
		V2DF  pos  = m_objects[i]->getPosition();
		float rot  = m_objects[i]->getRotation();
		V2DF  lvel = m_objects[i]->getLinearVelocity();
		float avel = m_objects[i]->getAngularVelocity();

		//clamp the linear force between 0 and defined max linear force
		clamp(m_objects[i]->refLinearForce(),0.0f,
			m_objects[i]->getMaxLinearForce());

		//clamp the angular force between 0 and defined max angular force
		clamp(m_objects[i]->refAngularForce(),0.0f,
			m_objects[i]->getMaxAngularForce());

		//update physics for current object
		calcPhysics(
			pos,rot,lvel,avel,
			m_objects[i]->getLinearForce(),
			m_objects[i]->getAngularForce(),
			m_objects[i]->getLinearDamping(),
			m_objects[i]->getAngularDamping(),
			m_objects[i]->getMass(),
			m_objects[i]->getInertia(),
			a_time);

		//set both the velocities along with the rotation and position
		//also generate a new bounding box with the new coordinates
		m_objects[i]->setLinearVelocity(lvel);
		m_objects[i]->setAngularVelocity(avel);
		m_objects[i]->setRotation(rot);
		m_objects[i]->setPosition(pos);
		m_objects[i]->m_aabb = genAABB(pos,m_objects[i]->getAVerts());

		//partition the object
		m_partition.partition(m_objects[i]);
	}
}

void clkWorld::generateContacts(float a_time)
{

}

void clkWorld::resolveContacts(float a_time)
{

}

void clkWorld::simulate(float a_time)
{
	m_partition.clearCells();
	for(int i = 0; i < m_objects.size(); i++)
	{
		V2DF  pos  = m_objects[i]->getPosition();
		float rot  = m_objects[i]->getRotation();
		float mass = m_objects[i]->getMass();
		float iner = m_objects[i]->getInertia();
		V2DF  lvel = m_objects[i]->getLinearVelocity();
		float avel = m_objects[i]->getAngularVelocity();
		float ldmp = m_objects[i]->getLinearDamping();
		float admp = m_objects[i]->getAngularDamping();

		m_objects[i]->refLinearForce().limit(
			m_objects[i]->getMaxLinearForce());

		if(m_objects[i]->getAngularForce() >
			m_objects[i]->getMaxAngularForce())
			m_objects[i]->refAngularForce() =
			m_objects[i]->getMaxAngularForce();
		else if(m_objects[i]->getAngularForce() <
			-m_objects[i]->getMaxAngularForce())
			m_objects[i]->refAngularForce() =
			-m_objects[i]->getMaxAngularForce();

		V2DF  lfrc = m_objects[i]->getLinearForce();
		float afrc = m_objects[i]->getAngularForce();

		calcPhysics(pos,rot,lvel,avel,lfrc,afrc,ldmp,admp,mass,iner,a_time);

		m_objects[i]->setRotation(rot);
		m_objects[i]->setPosition(pos);
		m_objects[i]->setLinearVelocity(lvel);
		m_objects[i]->setAngularVelocity(avel);
		m_objects[i]->m_aabb = genAABB(pos,m_objects[i]->getAVerts());

		m_partition.partition(m_objects[i]);
	}

	List<clkCell*>* cells = &m_partition.m_cells;
	for(int i = 0; i < cells->size(); i++)
	{
		clkCell* cell = cells->get(i);
		if(cell->size() <= 1) continue;
		for(int j = 0; j < cell->size(); j++)
		{
			clkObject* objA = cell->get(j);
			
			if(!objA->isCollidable()) continue;

			for(int k = 0; k < cell->size(); k++)
			{
				clkObject* objB = cell->get(k);

				if(objA == objB) continue;

				V2DF mtvAxis; float mtvMag = 0;
				if(satCollision(objA->getRVerts(),objA->getNorms(),
					objB->getRVerts(),objB->getNorms(),&mtvAxis,&mtvMag))
				{
					objA->refPosition() += (mtvAxis * mtvMag);
					objA->refLinearVelocity() += (mtvAxis * mtvMag) * a_time;
					objA->m_aabb = genAABB(objA->getPosition(),objA->getAVerts());
				}
			}
		}
	}
}

clkObject* clkWorld::createObject(clkBody* a_body, clkShape* a_shape)
{
	if(a_body == 0 || a_shape == 0) return 0;

	//create new object
	clkObject* obj = new clkObject;

	//create new body; fill with given body data
	clkBody* body = new clkBody;
	body->m_position   = a_body->m_position;
	body->m_rotation   = a_body->m_rotation;
	body->m_mass       = a_body->m_mass;
	body->m_inertia    = a_body->m_mass;
	body->m_lnVelocity = a_body->m_lnVelocity;
	body->m_agVelocity = a_body->m_agVelocity;
	body->m_lnDamping  = a_body->m_lnDamping;
	body->m_agDamping  = a_body->m_agDamping;
	body->m_maxLnForce = a_body->m_maxLnForce;
	body->m_maxAgForce = a_body->m_maxAgForce;
	body->m_collidable = a_body->m_collidable;
	body->m_static     = a_body->m_static;

	//give body to obj
	obj->m_body = body;

	//determine shape type
	switch(a_shape->m_type)
	{
	case SHAPE_CIRCLE:
		{
			//create new circle shape;
			//fill with given shape data
			clkCircle* shp = (clkCircle*)a_shape;
			clkCircle* cir = new clkCircle();
			cir->m_type    = SHAPE_CIRCLE;
			cir->m_radius  = shp->m_radius;
			cir->m_dir	   = shp->m_dir;

			//give circle shape to object
			obj->m_shape = cir;
		}
		break;
	case SHAPE_RECT:
		{
			//create new polygon shape in object;
			//fill with given shape data
			clkRect* shp      = (clkRect*)a_shape;
			clkRect* rect     = new clkRect();
			rect->m_type      = SHAPE_RECT;
			rect->m_vertCount = shp->m_vertCount;
			rect->m_dir	      = shp->m_dir;
			rect->m_dir	      = shp->m_dir;
			rect->m_width	  = shp->m_width;
			rect->m_height	  = shp->m_height;

			rotateVec(rect->m_dir,body->m_rotation);
			for(int i = 0; i < shp->m_vertCount; i++)
			{
				rect->m_averts[i] = shp->m_averts[i];
				rect->m_norms[i] = shp->m_norms[i];
				rotateVec(rect->m_averts[i],body->m_rotation);
				rotateVec(rect->m_norms[i],body->m_rotation);
				rect->m_rverts[i] = body->m_position+shp->m_rverts[i];
			}

			//generate AABB
			obj->m_aabb = genAABB(body->m_position,rect->m_averts);

			//give polygon shape to object
			obj->m_shape = rect;
		}
		break;
	}

	//check if obj is valid
	if(obj->m_body == 0 || obj->m_shape == 0)
	{
		SAFE_DELETE(obj->m_body);
		SAFE_DELETE(obj->m_shape);
		SAFE_DELETE(obj);
		return 0;
	}

	//add object to object list
	m_objects.insertBack(obj);

	//partition the object
	m_partition.partition(obj);

	//return create object
	return obj;
}

void clkWorld::getCellPos(List<V2DF*>* m_pos)
{
	m_partition.getCellPos(m_pos);
}